#include "zuc/nml_intf/zuc.hh"

extern int zucSetKineDHParams(DHParam dhParam);
extern int zucSetDynParams(DynParam dynParam);
extern int zucMotorSetRatedCurr(double* ratedCurr);
extern int zucMotorSetRatedTorq(double* ratedTorque);
extern int zucMotorSetRatedTransRatio(double* ratedTransRatio);
extern int zucAxisSetMinPositionLimit(int axis, double limit);
extern int zucAxisSetMaxPositionLimit(int axis, double limit);
extern int zucAxisSetMaxVelocity(int axis, double vel);
extern int zucAxisSetMaxAcceleration(int axis, double acc);
extern int zucAxisSetMaxVelocityRoboModel(int axis, double vel);
extern int zucAxisSetMaxAccelerationRoboModel(int axis, double acc);
extern int zucAxisSetClsnAccelerationRoboModel(double clsn_pos_acc, double clsn_ori_acc, double* clsn_jacc);
extern int zucAxisSetLockingJoint(int axis, int joint);
extern int zucJointSetType(int joint, unsigned char jointType);
extern int zucJointSetUnits(int joint, double units);
extern int zucJointSetBacklash(int joint, double backlash);
extern int zucJointSetFerror(int joint, double ferror);
extern int zucJointSetMinFerror(int joint, double ferror);
extern int zucJointSetHomingParams(int joint,
                                   double home,
                                   double offset,
                                   double home_vel,
                                   double search_vel,
                                   double latch_vel,
                                   int use_index,
                                   int ignore_limits,
                                   int is_shared,
                                   int home_sequence,
                                   int volatile_home,
                                   int locking_indexer,
                                   int absolute_encoder);
extern int zucJointUpdateHomingParams(int joint, double home, double offset);
extern int zucMotorsServoOn(bool on);
extern int zucJointInit(int joint);
extern int zucJointHalt(int joint);
extern int zucJointEnable(int joint);
extern int zucJointDisable(int joint);
extern int zucJointHome(int joint);
extern int zucJointUnhome(int joint);
extern int zucJointActivate(int joint);
extern int zucJointDeactivate(int joint);
extern int zucJointOverrideLimits(int joint);
extern int zucTrajSetJoints(int joints);
extern int zucTrajSetAxes(int axismask);
extern int zucTrajSetUnits(double linearUnits, double angularUnits);
extern int zucTrajInit();
extern int zucTrajHalt();
extern int zucInit();
extern int zucJointSetMaxAcceleration(int joint, double acc);
extern int zucTrajSetVelocity(double vel, double ini_maxvel);
extern int zucTrajSetAcceleration(double acc);
extern int zucTrajSetMaxVelocity(double vel);
extern int zucTrajSetMaxAcceleration(double acc);

int zucJointSetMaxAcceleration(int joint, double acc)
{
    return 0;
    // CATCH_NAN(std::isnan(acc));

    // if (joint < 0 || joint >= ZUCMOT_MAX_JOINTS)
    // {
    //     return 0;
    // }
    // if (acc < 0.0)
    // {
    //     acc = 0.0;
    // }
    // JointConfig[joint].MaxAccel = acc;

    // auto cmd = std::make_shared<CmdSetJointAccLimit>(0, joint, acc);
    // int retval = mot::realtime_cmd_call(cmd);
    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, joint, acc, retval);
    // }
    // return retval;
}

int zucJointEnable(int joint) { return 0; }

int zucJointDisable(int joint) { return 0; }

int zucJointHome(int joint)
{
    // if (joint < -1 || joint >= ZUCMOT_MAX_JOINTS)
    // {
    //     return 0;
    // }

    // if (zucmotStatus.protective_stop == 1)
    // {
    //     rcs_print("Cannot home joint when in protective_stop status\n");
    //     return -1;
    // }

    // auto cmd = std::make_shared<CmdJointHome>(0, joint);
    // return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucJointUnhome(int joint)
{
    // if (joint < -2 || joint >= ZUCMOT_MAX_JOINTS)
    // {
    //     return 0;
    // }

    // auto cmd = std::make_shared<CmdJointUnhome>(0, joint);
    // return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucMotorsServoOn(bool on)
{
    // if (on == false)
    // {
    //     auto cmd = std::make_shared<CmdMotorsServoOff>(0);
    //     return mot::realtime_cmd_call(cmd);
    // }
    // else
    // {
    //     auto cmd = std::make_shared<CmdMotorsServoOn>(0);
    //     return mot::realtime_cmd_call(cmd);
    // }
    return 0;
}

int zucJointOverrideLimits(int joint)
{
    // can have joint < 0, for resuming normal limit checking
    // if (joint >= ZUCMOT_MAX_JOINTS)
    // {
    //     return 0;
    // }
    // auto cmd = std::make_shared<CmdOverrideLimits>(0, joint);
    // return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucJointActivate(int joint)
{
    // if (joint < 0 || joint >= ZUCMOT_MAX_JOINTS)
    // {
    //     return 0;
    // }

    // auto cmd = std::make_shared<CmdJointActivate>(0, joint);
    // int retval = mot::realtime_cmd_call(cmd);

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%d) returned %d\n", __FUNCTION__, joint, retval);
    // }
    // return retval;
    return 0;
}

int zucJointDeactivate(int joint)
{
    // if (joint < 0 || joint >= ZUCMOT_MAX_JOINTS)
    // {
    //     return 0;
    // }

    // auto cmd = std::make_shared<CmdJointDeactivate>(0, joint);
    // return mot::realtime_cmd_call(cmd);
    return 0;
}

// 直接由配置文件同步，不存在用户设置
int zucsetJointOffsetAndDir(const double* offset, const int* dir)
{
    // for (int i = 0; i < ZUCMOT_MAX_JOINTS; i++) { zucStatus->motion.robotJointOffset[i] = offset[i]; }
    // auto cmd = std::make_shared<CmdSetJointOffsetAndDir>(0, offset, dir);
    // return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucAxisInit(int axis)
{
    return 0;
    // int retval = 0;

    // if (axis < 0 || axis >= ZUCMOT_MAX_AXIS)
    // {
    //     return 0;
    // }
    // // init zucmot interface
    // if (!JointOrTrajInited())
    // {
    //     if (0 != usrmotInit("zuc_task"))
    //     {
    //         return -1;
    //     }
    // }
    // AxisConfig[axis].Inited = 1;
    // if (0 != iniAxis(axis, zuc_inifile))
    // {
    //     retval = -1;
    // }
    // return retval;
}

// 由motion_al进行初始化，其中的很多状态是一个机器人一份
int zucTrajInit()
{
    return 0;
    // toppraPyEnvInit();

    // int retval = 0;
    // // RobotModelData robotModelData;
    // TrajConfig.Inited = 0;
    // TrajConfig.Joints = 0;
    // TrajConfig.MaxAccel = DBL_MAX;
    // TrajConfig.DeprecatedAxes = 0;
    // TrajConfig.AxisMask = 0;
    // TrajConfig.LinearUnits = 1.0;
    // TrajConfig.AngularUnits = 1.0;
    // TrajConfig.MotionId = 0;
    // TrajConfig.MaxVel = DEFAULT_TRAJ_MAX_VELOCITY;

    // // init zucmot interface
    // // @FIXME 重复代码，这个主函数前已经初始化过一次
    // if (!JointOrTrajInited())
    // {
    //     if (0 != usrmotInit("zuc_task"))
    //     {
    //         rtapi_print("usrmotInit(zuc_task) failed\n\n");
    //         return -1;
    //     }
    // }
    // TrajConfig.Inited = 1;

    // // initialize parameters from ini file
    // if (0 != iniTraj(zuc_inifile))
    // {
    //     rtapi_print("iniTraj failed\n\n");
    //     retval = -1;
    // }

    // // // @FIXME 与initOnekeyset函数作用相同，可以整合为一个函数
    // // if (0 != zucInitAutoOp(zuc_inifile))
    // // {
    // //     rtapi_print("iniAutoSetting failed\n\n");
    // //     retval = -1;
    // // }
    // // else
    // // {
    // //     rtapi_print("***************iniAutoSetting successed***************\n\n");
    // //     // rtapi_print("***************zucStatus->autoopsetting.autoenable = %d***************\n\n", zucStatus->autoopsetting.onekeyenable);
    // //     // rtapi_print("***************zucStatus->autoopsetting.autoprogram = %d***************\n\n", zucStatus->autoopsetting.autoprogram);
    // // }

    // // 优先读取机型文件，确认机器人轴数量
    // if (0 != zucInitRobot(zuc_inifile))
    // {
    //     rcs_print("KineDH init failed!\n");
    //     retval = -1;
    // }

    // // 与机器人轴数相关的配置读取
    // if (0 != initSetAboutJoint(zuc_inifile, zucStatus->motion.robot_joint_num))
    // {
    //     rtapi_print("initSetAboutJoint failed\n\n");
    //     retval = -1;
    // }
    // zucInitCustomPose(zuc_inifile);
    // zucInitJointSafetyLimitSettings(zuc_inifile);

    // //初始化并加载关节统计数据
    // ServoStatistic::get()->init(zucStatus->motion.robot_serial_num);
    // // ZucIniFile _trajInifile;
    // // _trajInifile.Open(zuc_inifile);
    // Util::IniParser _ini_file(zuc_inifile);
    // ServoStatistic::get()->load(&_ini_file);

    // if (0 != zucInitFuncIOMap(zuc_inifile))
    // {
    //     rcs_print("FuncIO init failed!\n");
    //     retval = -1;
    // }

    // if (zuc_cab_type >= 2)
    // {
    //     zucInitSafetyFuncIOSettings(zuc_inifile);
    //     zucInitSafetyRobotPowerLimitSettings(zuc_inifile);
    // }

    // zucInitTIOSetup(zuc_inifile);

    // // 传感器硬件相关设置
    // zucInitTorqSensorSetup(zuc_inifile);

    // zucInitRobotSafetyLimitSettings(zuc_inifile);
    // zucInitRobotReduceSafetyLimitSettings(zuc_inifile);
    // zucInitNetExceptionHandle(zuc_inifile);
    // zucInitDragSpeedLimitSettings(zuc_inifile);

    // return retval;
}

int zucJointSetMinFerror(int joint, double ferror) { return 0; }

int zucJointSetHomingParams(int joint,
                            double home,
                            double offset,
                            double home_final_vel,
                            double search_vel,
                            double latch_vel,
                            int use_index,
                            int ignore_limits,
                            int is_shared,
                            int sequence,
                            int volatile_home,
                            int locking_indexer,
                            int absolute_encoder)
{
    return 0;
    // #ifdef ISNAN_TRAP
    //     if (std::isnan(home) || std::isnan(offset) || std::isnan(home_final_vel) || std::isnan(search_vel) || std::isnan(latch_vel))
    //     {
    //         printf("isnan error in zucJointSetHomingParams()\n");
    //         return -1;
    //     }
    // #endif

    //     if (joint < 0 || joint >= ZUCMOT_MAX_JOINTS)
    //     {
    //         return 0;
    //     }

    //     int homingflag = 0;
    //     if (use_index)
    //     {
    //         homingflag |= HOME_USE_INDEX;
    //     }
    //     if (ignore_limits)
    //     {
    //         homingflag |= HOME_IGNORE_LIMITS;
    //     }
    //     if (is_shared)
    //     {
    //         homingflag |= HOME_IS_SHARED;
    //     }
    //     if (locking_indexer)
    //     {
    //         homingflag |= HOME_UNLOCK_FIRST;
    //     }
    //     if (absolute_encoder)
    //     {
    //         switch (absolute_encoder)
    //         {
    //         case 0:
    //             break;
    //         case 1:
    //             homingflag |= HOME_ABSOLUTE_ENCODER;
    //             homingflag |= HOME_NO_REHOME;
    //             break;
    //         case 2:
    //             homingflag |= HOME_ABSOLUTE_ENCODER;
    //             homingflag |= HOME_NO_REHOME;
    //             homingflag |= HOME_NO_FINAL_MOVE;
    //             break;
    //         default:
    //             fprintf(stderr, "Unknown option for absolute_encoder <%d>", absolute_encoder);
    //             break;
    //         }
    //     }

    //     auto cmd = std::make_shared<CmdSetJointHomingParams>(0, joint, volatile_home, latch_vel, home_final_vel, home, search_vel, offset, homingflag, sequence);
    //     int retval = mot::realtime_cmd_call(cmd);

    //     if (zuc_debug & ZUC_DEBUG_CONFIG)
    //     {
    //         rcs_print("%s(%d, %.4f, %.4f, %.4f, %.4f, %.4f, %d, %d, %d, %d, %d) returned %d\n",
    //                   __FUNCTION__,
    //                   joint,
    //                   home,
    //                   offset,
    //                   home_final_vel,
    //                   search_vel,
    //                   latch_vel,
    //                   use_index,
    //                   ignore_limits,
    //                   is_shared,
    //                   sequence,
    //                   volatile_home,
    //                   retval);
    //     }
    //     return retval;
}

int zucJointSetFerror(int joint, double ferror)
{
    // #ifdef ISNAN_TRAP
    //     if (std::isnan(ferror))
    //     {
    //         printf("isnan error in zucJointSetFerror()\n");
    //         return -1;
    //     }
    // #endif

    //     if (joint < 0 || joint >= ZUCMOT_MAX_JOINTS)
    //     {
    //         return 0;
    //     }

    //     zucmotCommand.command = ZUCMOT_SET_JOINT_MAX_FERROR;
    //     zucmotCommand.joint = joint;
    //     // zucmotCommand.maxFerror = ferror;
    //     // zucmotCommand.jointsetferror.joint = joint;
    //     zucmotCommand.jointsetferror.maxFerror = ferror;

    //     int retval = usrmotWriteZucmotCommand(&zucmotCommand);

    //     if (zuc_debug & ZUC_DEBUG_CONFIG)
    //     {
    //         rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, joint, ferror, retval);
    //     }
    return 0;
}

int zucJointUpdateHomingParams(int joint, double home, double offset)
{
    return 0;
    // CATCH_NAN(std::isnan(home) || std::isnan(offset));

    // if (joint < 0 || joint >= ZUCMOT_MAX_JOINTS)
    // {
    //     return 0;
    // }

    // auto cmd = std::make_shared<CmdUpdateJointHomingParams>(0, joint, offset, home);
    // int retval = mot::realtime_cmd_call(cmd);

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%d, %.4f, %.4f) returned %d\n", __FUNCTION__, joint, home, offset, retval);
    // }
    // return retval;
}

int zucAxisSetMinPositionLimit(int axis, double limit)
{
    return 0;
    // CATCH_NAN(std::isnan(limit));

    // if (axis < 0 || axis >= ZUCMOT_MAX_AXIS || !(TrajConfig.AxisMask & (1 << axis)))
    // {
    //     return 0;
    // }

    // AxisConfig[axis].MinLimit = limit;

    // auto cmd = std::make_shared<CmdSetAxisPositionLimits>(0, axis, AxisConfig[axis].MaxLimit, AxisConfig[axis].MinLimit);
    // int retval = mot::realtime_cmd_call(cmd);

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, axis, limit, retval);
    // }
    // return retval;
}

int zucAxisSetMaxPositionLimit(int axis, double limit)
{
    return 0;
    // CATCH_NAN(std::isnan(limit));

    // if (axis < 0 || axis >= ZUCMOT_MAX_AXIS || !(TrajConfig.AxisMask & (1 << axis)))
    // {
    //     return 0;
    // }

    // AxisConfig[axis].MaxLimit = limit;

    // auto cmd = std::make_shared<CmdSetAxisPositionLimits>(0, axis, AxisConfig[axis].MaxLimit, AxisConfig[axis].MinLimit);
    // int retval = mot::realtime_cmd_call(cmd);

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, axis, limit, retval);
    // }
    // return retval;
}

int zucAxisSetLockingJoint(int axis, int joint)
{
    return 0;
    // if (axis < 0 || axis >= ZUCMOT_MAX_AXIS || !(TrajConfig.AxisMask & (1 << axis)))
    // {
    //     return 0;
    // }

    // if (joint < 0)
    // {
    //     joint = -1;
    // }

    // auto cmd = std::make_shared<CmdSetAxisLockingJoint>(0, joint, axis);
    // int retval = mot::realtime_cmd_call(cmd);

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%d, %d) returned %d\n", __FUNCTION__, axis, joint, retval);
    // }
    // return retval;
}

/*! functions involving carthesian Axes (X,Y,Z,A,B,C,U,V,W) */

int zucAxisSetMaxVelocity(int axis, double vel)
{
    return 0;
    // CATCH_NAN(std::isnan(vel));

    // if (axis < 0 || axis >= ZUCMOT_MAX_AXIS || !(TrajConfig.AxisMask & (1 << axis)))
    // {
    //     return 0;
    // }

    // if (vel < 0.0)
    // {
    //     vel = 0.0;
    // }
    // double modelMaxVel = 0.0;
    // if (axis < 3)
    // {
    //     modelMaxVel = zucStatus->motion.robot_model_data.maxTrajVel;
    // }
    // else
    // {
    //     modelMaxVel = zucStatus->motion.robot_model_data.maxTrajVel_ori;
    // }

    // if (modelMaxVel > 1e-3 && modelMaxVel < vel)
    // {
    //     rtapi_print(" [axis set max vel]: MaxVel = %f, vel = %f\n", modelMaxVel, vel);
    //     vel = modelMaxVel;
    // }

    // AxisConfig[axis].MaxVel = vel;

    // auto cmd = std::make_shared<CmdSetAxisVelLimit>(0, axis, vel);
    // int retval = mot::realtime_cmd_call(cmd);

    // iniSaveAxisMaxVel(axis, vel, zuc_inifile);

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, axis, vel, retval);
    // }
    // return retval;
}

int zucAxisSetMaxVelocityRoboModel(int axis, double vel)
{
    return 0;
    // CATCH_NAN(std::isnan(vel));

    // if (axis < 0 || axis >= ZUCMOT_MAX_AXIS || !(TrajConfig.AxisMask & (1 << axis)))
    // {
    //     return 0;
    // }

    // if (vel < 0.0)
    // {
    //     vel = 0.0;
    // }

    // AxisConfig[axis].MaxVel = vel;

    // auto cmd = std::make_shared<CmdSetAxisVelLimit>(0, axis, vel);
    // int retval = mot::realtime_cmd_call(cmd);

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("RoboModel %s(%d, %.4f) returned %d\n", __FUNCTION__, axis, vel, retval);
    // }
    // return retval;
}

int zucAxisSetMaxAcceleration(int axis, double acc)
{
    return 0;
    // CATCH_NAN(std::isnan(acc));

    // if (axis < 0 || axis >= ZUCMOT_MAX_AXIS || !(TrajConfig.AxisMask & (1 << axis)))
    // {
    //     return 0;
    // }

    // if (acc < 0.0)
    // {
    //     acc = 0.0;
    // }

    // double modelMaxAcc = 0.0;
    // if (axis < 3)
    // {
    //     modelMaxAcc = zucStatus->motion.robot_model_data.maxTrajAcc;
    // }
    // else
    // {
    //     modelMaxAcc = zucStatus->motion.robot_model_data.maxTrajAcc_ori;
    // }

    // if (modelMaxAcc > 1e-3 && modelMaxAcc < acc)
    // {
    //     rtapi_print(" [axis set max vel]: modelMaxAcc = %f, acc = %f\n", modelMaxAcc, acc);
    //     acc = modelMaxAcc;
    // }

    // AxisConfig[axis].MaxAccel = acc;

    // auto cmd = std::make_shared<CmdSetAxisAccLimit>(0, axis, acc);
    // int retval = mot::realtime_cmd_call(cmd);

    // iniSaveAxisMaxAcc(axis, acc, zuc_inifile);

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%d, %.4f) returned %d\n", __FUNCTION__, axis, acc, retval);
    // }
    // return retval;
}

int zucAxisSetMaxAccelerationRoboModel(int axis, double acc)
{
    return 0;
    // CATCH_NAN(std::isnan(acc));

    // if (axis < 0 || axis >= ZUCMOT_MAX_AXIS || !(TrajConfig.AxisMask & (1 << axis)))
    // {
    //     return 0;
    // }

    // if (acc < 0.0)
    // {
    //     acc = 0.0;
    // }

    // AxisConfig[axis].MaxAccel = acc;

    // auto cmd = std::make_shared<CmdSetAxisAccLimit>(0, axis, acc);
    // int retval = mot::realtime_cmd_call(cmd);

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("RoboModel %s(%d, %.4f) returned %d\n", __FUNCTION__, axis, acc, retval);
    // }
    // return retval;
}

int zucAxisSetClsnAccelerationRoboModel(double clsn_pos_acc, double clsn_ori_acc, double* clsn_jacc)
{
    return 0;
    // if (clsn_jacc == nullptr)
    //     return -1;

    // CATCH_NAN(std::isnan(clsn_pos_acc) || std::isnan(clsn_ori_acc));

    // if (clsn_pos_acc < 0.0)
    // {
    //     rtapi_print("[zucAxisSetClsnAccelerationRoboModel] : clsn_pos_acc = %f < 0.0\n", clsn_pos_acc);
    //     clsn_pos_acc = 8000.0;
    // }

    // if (clsn_ori_acc < 0.0)
    // {
    //     rtapi_print("[zucAxisSetClsnAccelerationRoboModel] : clsn_ori_acc = %f < 0.0\n", clsn_ori_acc);
    //     clsn_ori_acc = 1080.0;
    // }
    // for (int jnum = 0; jnum < ZUCMOT_MAX_JOINTS; jnum++)
    // {
    //     if (clsn_jacc[jnum] < 0.0)
    //     {
    //         rtapi_print("[zucAxisSetClsnAccelerationRoboModel] : clsn_ori_clsn_jacc[%d]acc = %f < 0.0\n", jnum, clsn_jacc[jnum]);
    //         clsn_jacc[jnum] = 1080.0;
    //     }
    // }

    // auto cmd = std::make_shared<CmdSetClsnAccLimit>(0, clsn_pos_acc, clsn_ori_acc);
    // int retval = mot::realtime_cmd_call(cmd);

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("RoboModel %s(%.4f, %.4f, %.4f) returned %d\n", __FUNCTION__, clsn_pos_acc, clsn_ori_acc, clsn_jacc[0], retval);
    // }
    // return retval;
}

double zucAxisGetMaxVelocity(int axis)
{
    return 0;
    // if (axis < 0 || axis >= ZUCMOT_MAX_AXIS)
    // {
    //     return 0;
    // }

    // return AxisConfig[axis].MaxVel;
}

/*! functions involving joints */
int zucJointSetType(int joint, unsigned char jointType)
{
    // // @FIXME 默认设置为了旋转关节
    // if (joint < 0 || joint >= ZUCMOT_MAX_JOINTS)
    // {
    //     return 0;
    // }

    // JointConfig[joint].Type = jointType;

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%d, %d)\n", __FUNCTION__, joint, jointType);
    // }
    return 0;
}

double zucTrajGetLinearUnits()
{
    return 0;  //TrajConfig.LinearUnits;
}

double zucTrajGetAngularUnits()
{
    return 0;  //TrajConfig.AngularUnits;
}

int zucJointSetUnits(int joint, double units)
{
    // if (joint < 0 || joint >= ZUCMOT_MAX_JOINTS)
    // {
    //     return 0;
    // }

    // JointConfig[joint].Units = units;

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%d, %.4f)\n", __FUNCTION__, joint, units);
    // }
    return 0;
}

int zucJointSetBacklash(int joint, double backlash) { return 0; }

static int JointOrTrajInited(void)
{
    // int joint;

    // for (joint = 0; joint < ZUCMOT_MAX_JOINTS; joint++)
    // {
    //     if (JointConfig[joint].Inited)
    //     {
    //         return 1;
    //     }
    // }
    // if (TrajConfig.Inited)
    // {
    //     return 1;
    // }
    return 0;
}

int zucJointInit(int joint)
{
    int retval = 0;
    // if (joint < 0 || joint >= ZUCMOT_MAX_JOINTS)
    // {
    //     return 0;
    // }
    // // @FIXME 重复操作
    // // init zucmot interface
    // if (!JointOrTrajInited())
    // {
    //     if (0 != usrmotInit("zuc_task"))
    //     {
    //         return -1;
    //     }
    // }
    // JointConfig[joint].Inited = 1;
    // if (0 != iniJoint(joint, zuc_inifile))
    // {
    //     retval = -1;
    // }
    return retval;
}

int zucJointHalt(int joint)
{
    // if (joint < 0 || joint >= ZUCMOT_MAX_JOINTS)
    // {
    //     return 0;
    // }
    // /*! \todo FIXME-- refs global zucStatus; should make ZUC_JOINT_STAT an arg here */
    // if (NULL != zucStatus && zucmotion_initialized && JointConfig[joint].Inited)
    // {
    //     //dumpJoint(joint, zuc_inifile, &zucStatus->motion.joint[joint]);
    // }
    // JointConfig[joint].Inited = 0;

    // if (!JointOrTrajInited())
    // {
    //     usrmotExit();  // ours is final exit
    // }
    return 0;
}

int zucTrajHalt()
{
    // TrajConfig.Inited = 0;

    // if (!JointOrTrajInited())
    // {
    //     usrmotExit();  // ours is final exit
    // }
    return 0;
}

int zucMotorSetRatedCurr(double* ratedCurr)
{
    // if (ratedCurr == nullptr)
    //     return -1;

    // auto cmd = std::make_shared<CmdSetMotorRatedcurr>(0, ratedCurr);
    // return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucMotorSetRatedTorq(double* ratedTorque)
{
    // if (ratedTorque == nullptr)
    //     return -1;

    // auto cmd = std::make_shared<CmdSetMotorRatedtorq>(0, ratedTorque);
    // return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucMotorSetRatedTransRatio(double* ratedTransRatio)
{
    // if (ratedTransRatio == nullptr)
    //     return -1;

    // auto cmd = std::make_shared<CmdSetMotorRatedtransratio>(0, ratedTransRatio);
    // return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetRobotModel(int robot_model)
{
    return 0;
    // setKineRobotSeries(robot_model / 10000);

    // auto cmd = std::make_shared<CmdSetRobotModel>(0, robot_model);
    // return mot::realtime_cmd_call(cmd);
}

int zucTrajSetJoints(int joints)
{
    // if (joints <= 0 || joints > ZUCMOT_MAX_JOINTS)
    // {
    //     rcs_print("zucTrajSetJoints failing: joints=%d\n", joints);
    //     return -1;
    // }

    // TrajConfig.Joints = joints;
    // auto cmd = std::make_shared<CmdSetNumJoints>(0, joints);
    // int retval = mot::realtime_cmd_call(cmd);

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%d) returned %d\n", __FUNCTION__, joints, retval);
    // }
    return 0;
}

int zucTrajSetAxes(int axismask)
{
    // int axes = 0;
    // for (int i = 0; i < ZUCMOT_MAX_AXIS; i++)
    //     if (axismask & (1 << i))
    //         axes = i + 1;

    // TrajConfig.DeprecatedAxes = axes;
    // TrajConfig.AxisMask = axismask;

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%d, %d)\n", __FUNCTION__, axes, axismask);
    // }
    return 0;
}

int zucTrajSetUnits(double linearUnits, double angularUnits)
{
    // 默认为mm，deg
    // if (linearUnits <= 0.0 || angularUnits <= 0.0)
    // {
    //     return -1;
    // }

    // TrajConfig.LinearUnits = linearUnits;
    // TrajConfig.AngularUnits = angularUnits;

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%.4f, %.4f)\n", __FUNCTION__, linearUnits, angularUnits);
    // }
    return 0;
}

int zucSetDynParams(DynParam dynParam)
{
    return 0;
    // int i = 0;
    // for (i = 0; i < 70; i++) { dynParameters[i] = dynParam.kineticK[i]; }                              // @FIXME liwang 需优化
    // for (i = 0; i < zucStatus->motion.robot_joint_num; i++) { vf_params[i] = dynParameters[i + 42]; }  // @FIXME liwang 需优化

    // DynPara_init_toppra(toolParameters, install_angle, dynParameters, dh_a, dh_d, dh_alpha);  // @FIXME liwang DH与7轴不符合
    // auto cmd = std::make_shared<CmdSetDynParam>(0, dynParam);
    // return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucSetKineDHParams(DHParam dhParam)
{
    // zucTaskDHParam = dhParam;
    // setInterpDHParam(zucTaskDHParam);  // @FIXME liwang DH与7轴不符合
    // // iniSaveDHParam(zucStatus->motion.robot_serial_num, zucTaskDHParam, zuc_inifile);

    // // zucmotCommand.dhParam = zucTaskDHParam;
    // for (int i = 0; i < zucStatus->motion.robot_joint_num; i++)
    // {
    //     dh_a[i] = zucTaskDHParam.a[i] / 1000.0;
    //     dh_d[i] = zucTaskDHParam.d[i] / 1000.0;
    //     dh_alpha[i] = zucTaskDHParam.alpha[i] * PM_PI / 180.0;
    // }
    // DynPara_init_toppra(toolParameters, install_angle, dynParameters, dh_a, dh_d, dh_alpha);
    // auto cmd = std::make_shared<CmdSetDhParam>(0, dhParam);
    // return mot::realtime_cmd_call(cmd);
    return 0;
}

int zucTrajSetVelocity(double vel, double ini_maxvel) /*  set the velocity for subsequent moves  */
{
    return 0;
    // 意义不明
    // if (vel < 0.0)
    // {
    //     vel = 0.0;
    // }
    // else if (vel > TrajConfig.MaxVel)
    // {
    //     vel = TrajConfig.MaxVel;
    // }

    // if (ini_maxvel < 0.0)
    // {
    //     ini_maxvel = 0.0;
    // }
    // else if (vel > TrajConfig.MaxVel)
    // {
    //     ini_maxvel = TrajConfig.MaxVel;
    // }

    // auto cmd = std::make_shared<CmdSetVel>(0, ini_maxvel, vel);
    // int retval = mot::realtime_cmd_call(cmd);

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%.4f, %.4f) returned %d\n", __FUNCTION__, vel, ini_maxvel, retval);
    // }
    // return retval;
}

int zucTrajSetAcceleration(double acc) /* set the max accel for moves (tooltip) */
{
    return 0;
    // if (acc < 0.0)
    // {
    //     acc = 0.0;
    // }
    // else if (acc > TrajConfig.MaxAccel)
    // {
    //     acc = TrajConfig.MaxAccel;
    // }

    // auto cmd = std::make_shared<CmdSetAcc>(0, acc);
    // int retval = mot::realtime_cmd_call(cmd);

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%.4f) returned %d\n", __FUNCTION__, acc, retval);
    // }
    // return retval;
}

/*
  zucmot has no limits on max velocity, acceleration so we'll save them
  here and apply them in the functions above
  */
int zucTrajSetMaxVelocity(double vel)
{
    return 0;
    // 意义不明
    // if (vel < 0.0)
    // {
    //     vel = 0.0;
    // }

    // TrajConfig.MaxVel = vel;

    // auto cmd = std::make_shared<CmdSetVelLimit>(0, vel);
    // int retval = mot::realtime_cmd_call(cmd);

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%.4f) returned %d\n", __FUNCTION__, vel, retval);
    // }
    // return retval;
}

int zucTrajSetMaxAcceleration(double acc)
{
    // if (acc < 0.0)
    // {
    //     acc = 0.0;
    // }

    // TrajConfig.MaxAccel = acc;

    // if (zuc_debug & ZUC_DEBUG_CONFIG)
    // {
    //     rcs_print("%s(%.4f)\n", __FUNCTION__, acc);
    // }
    return 0;
}